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1  Statement  of  Problem:  Automation  of  Helicopter 
Formations 

Formation  flight  is  the  primary  movement  technique  for  teams  of  helicopters. 
However,  the  potential  for  accidents  is  greatly  increased  when  helicopter 
teams  are  required  to  fly  in  tight  formations  and  under  harsh  conditions. 
ARO  STIR  W911NF-04-1-0448  proposed  that  the  automation  of  helicopter 
formations  is  a  realistic  solution  capable  of  alleviating  risks.  Helicopter 
formation  flight  operations  in  battlefield  situations  are  highly  dynamic  and 
dangerous,  and,  therefore,  we  maintain  that  both  a  high-level  formation 
management  system  and  a  distributed  coordinated  control  algorithm  should 
be  implemented  to  help  ensure  safe  flight  formations. 

Our  ultimate  research  objective  is  to  develop  technologies  that  work 
both  manned  and  unmanned  rotorcraft,  since  it  is  our  sense  that  they  will 
be  working  together  in  the  future.  We  also  deeply  believe  that  it  is  important 
to  develop  tools  for  human  centered  automation  as  a  via-point  to  complete 
automation.  As  a  preliminary  work  toward  autonomous  (for  unmanned 
rotorcraft)  and  semi-autonomous  (for  manned  helicopters)  formation  flight 
control  and  management  system,  we  designed  decentralized  nonlinear  model 
predictive  control  (MPC)  law  for  coupled  systems.  Also,  a  hardware-in-the- 
loop  simulation  (HILS)  system  was  implemented  as  the  future  development 
tool. 

The  starting  point  for  ARO  STIR  W911NF-04-1-0448  was  to  design  a 
distributed  control  law  attenuating  external  disturbances  coming  into  a  for¬ 
mation,  so  that  each  vehicle  can  safely  maintain  sufficient  space  between  it 
and  all  other  vehicles.  While  conventional  methods  are  limited  to  homoge¬ 
neous  formations,  our  decentralized  MPC  approach  allows  for  heterogeneity 
in  a  formation.  In  order  to  avoid  the  conservative  nature  inherent  in  dis¬ 
tributed  MPC  algorithms,  we  begin  by  designing  a  stable  MPC  for  individual 
vehicles,  and  then  introducing  carefully  designed  inter- agent  coupling  terms 
in  a  performance  index.  Thus  the  proposed  algorithm  works  in  a  decentral¬ 
ized  manner,  and  can  be  applied  to  the  problem  of  safe  helicopter  formations 
comprised  of  heterogenous  vehicles. 

In  order  to  perform  formation  flight  experiments  based  on  the  proposed 
algorithm  wrhile  minimizing  the  possibility  of  software  failure,  we  developed 
a  hardware-in-the-loop  simulation  (HILS)  system  which  is  compatible  with 
our  rotorcraft- based  unmanned  aerial  vehicle  (RUAV)  systems.  Using  mul¬ 
tiple  HILS  system  enables  us  to  verify  newly  developed  code  in  realistic 
circumstances.  Also,  the  developed  HILS  system  will  be  used  as  a  virtual 
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helicopter  in  the  future  experiments. 

Section  2  describes  linear  helicopter  dynamics  and  nonlinear  kinemat¬ 
ics,  and  a  cruise  model  of  the  Yamaha  R-50  industrial  unmanned  helicopter 
23],  which  is  used  throughout  this  report.  We  introduce  a  scaled-up  model 
by  talking  advantage  of  dimensional  analysis  for  the  heterogeneous  forma¬ 
tion  used  in  Section  3.  In  Section  3,  the  MPC  algorithm  for  autonomous 
helicopter  formations  is  formulated.  Decoupled  MPC  algorithms  for  individ¬ 
ual  vehicles  are  designed  based  on  the  Control  Lyapunov  Function  (CLF) 
approach  [17].  The  formation  topology  is  defined,  and  the  varying  gap 
strategy  and  the  constant  gap  strategy  are  introduced.  Through  computer 
simulations,  we  show  that  the  proposed  MPC  scheme  successfully  attenu¬ 
ates  external  disturbance  propagation  even  in  a  heterogeneous  formation. 
Section  4  describes  the  hardware  and  software  structure  of  the  HILS  system 
for  Berkeley  Aerobot  (BEAR)  fleet.  A  summation  of  work  completed  on 
ARO  STIR  W911NF-04-1-0448  is  given  in  Section  5  followed  by  conclusions 
and  suggestions  for  future  work. 

2  Helicopter  Dynamics 

Helicopter  dynamics  with  a  nonlinear  kinematics  model  in  a  general  form  is 
presented.  Based  on  this,  the  combined  dynamics  and  kinematics  model  for 
the  forward  cruise  flight  mode  is  derived  in  the  section  2.2. 

We  need  a  set  of  heterogeneous  helicopter  cruise  models  for  simulations, 
since  our  algorithm  proposed  in  the  next  section  is  focused  on  the  distur¬ 
bance  attenuation  property  in  a  heterogeneous  formation.  Because  a  cruise 
model  of  a  helicopter  with  proper  size  is  extremely  rare  in  the  literature,  we 
simply  use  a  dimensional  analysis  technique  presented  in  [23]  to  generate  a 
scaled-up  virtual  model.  Details  of  how  the  model  is  scaled  up  are  discussed 
in  Section  2.3. 

2.1  Helicopter  Dynamics  with  Nonlinear  Kinematics 

Since  the  helicopter  dynamics,  which  can  be  derived  using  Newton’s  law,  are 
represented  in  the  body  coordinates  system  fixed  to  the  center  of  the  mass 
of  a  helicopter  32],  the  kinematic  equations  between  the  body  coordinates 
and  the  spatial  coordinates1  are  required.  The  kinematics  are  further  divided 
into  two  parts:  (1)  the  position  describing  translational  motion  in  the  spatial 

1  Throughout  this  report,  the  spatial  coordinates  mean  the  tangent-plane  coordinate 
system,  whose  origin  is  located  at  a  certain  point  of  the  earth’s  surface. 
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Figure  1:  Free  body  diagram  of  a  helicopter  [32] 


coordinates,  and  (2)  the  Euler  angles  describing  the  vehicle’s  attitude  and 
heading  in  the  spatial  coordinates. 

The  following  definitions  of  helicopter  dynamics  and  kinematics  are  based 
on  32,  34]  with  slight  modifications.  Figure  1  shows  the  body-fixed  coordi¬ 
nates,  forces  and  moments  exerted  on  the  helicopter.  The  positive  directions 
of  x,  y ,  and  2  axes  of  the  spatial  coordinates  align,  respectively,  to  the  north, 
east  and  downward  directions.  For  detailed  derivations  of  the  helicopter  dy¬ 
namics,  see  [32]  and  [23]. 

As  mentioned  earlier,  the  overall  system  dynamics  are  divided  into  the 
kinematics  and  the  system- specific  dynamics  denoted  by  superscripts  K  and 
D .  The  state  vectors  and  the  control  input  vector  are  defined  as  follows: 


xD  =  [u  v  p  q  au  bis  w  r  rfb 


XA  = 

V 

6 

,  -x-K  = 

|x*l 

0 

_i>_ 

U  —  [u»ls  '^als  ^Ve/]  i 


(1) 

(2) 

(3) 
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where 


u,v,w:  trimmed  translational  velocities  in  body  coordinates 

p:  g,  r:  roll,  pitch  and  yaw  rates  in  the  body  coordinates 

<j> ,  6 ,  i/;:  denote  roll,  pitch,  and  yaw  in  ZYX  Euler  angle  notation2  in  the 
spatial  coordinates 

ai bi9:  longitudinal  and  lateral  flapping  angles  of  the  main  rotor 

c,  d:  longitudinal  and  lateral  flapping  angles  of  the  Bell-Hiller  stabilizer 
bar 


rf(>:  internal  state  of  feedback  gyro 

uau,  Uais-  inputs  to  the  lateral  and  longitudinal  cyclic  pitch 


u$M :  input  to  the  main  rotor  collective  pitch 

uref:  reference  yaw  rate  input  to  the  gyro 

Let  the  superscripts  S  and  B  denote  spatial  and  body  coordinates.  xs  and 
xB  denote  the  position  in  the  spatial  coordinates  and  in  the  body  coordi¬ 
nates,  respectively3. 

The  kinematics  part  can  be  defined  as  follows: 


Xs  =  RB~ 


x.A  =  RE~*Sw, 


(4) 


where  xB  =  xs  ys  z^]r,  and  to  =  \p  q  r]T.  RB^S  (xA).  the  rotation  matrix 
from  the  body  to  the  spatial  coordinates,  and  RB~'s(xA)  is  the  relationship 
matrix  between  angular  rates  in  the  body  and  in  the  spatial  coordinates. 
They  are  defined  by  [6,  20] 


rb-s 


R^S 


cos  ip  cos  8  cos  ip  sin  8  sin  (p  —  simp  cos  <p 
sin  ip  cos  9  sin  ip  sin  8  sin  <p  4-  cos  ip  cos  p> 
—  sin  8  cos  8  sin  (p 

1  sin  4>  tan  8  cos  <p  tan  8 
0  cos  <p  —sirup 
0  sin  <p  sec  8  cos  <p  sec  8 


cos  ip  sin  8  cos  (p  4-  sin  ip  sin  <p 
sin  'tp  sin  8  cos  <j>  —  cos  ip  sin  (p 
cos  0  cos  <p 

(5) 

(6) 


2Rotate  'ip  in  Z,  8  in  Y'  and  <p  *n  X".  Note  that  this  transformation  results  in  same 
transformation  with  XYZ  fixed  angles  [fi] 

3subscript  vehicle  indexes  are  suppressed  until  the  next  section  for  simplicity 
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(7) 


The  dynamics  part  can  be  written  as 

xD  =  fD(xD(t),xA(t),n(t)). 
Finally,  the  entire  system  equation  becomes 


d_ 

dt 


or  simply, 


-XC1 

~fD(x.D(t),  ^(t),  u(t))" 

0 

= 

.Xs. 

Rb^sxb 

=  /(x5(t),  Xp(t),  xA(t),  u(i)),  (8) 


rx°' 


x(t)  5=  /(x(t),  u(t))  with  x  = 


(9) 


2.2  Helicopter  Cruise  Model 

In  [23],  a  linear  cruise  flight  model  of  the  Yamaha  R-50  industrial  helicopter 
trimmed  at  tto  =  49  ft /sec,  wq  =  1L2  ft/sec,  and  vq>  =  0  is  introduced  and  all 
the  coefficients  in  the  dynamics  equation  axe  identified  through  test  flights 
and  system  identification  techniques.  If  we  convert  this  trim  condition  into 
spatial  coordinates,  then  it  becomes  30  mi/h  forward  cruise  speed  with  the 
pitch  trim  #o  =  — 0.22(rad). 

In  order  to  use  the  linear  cruise  model,  we  need  to  define  several  relation¬ 
ships  between  spatial  variables  and  variables  in  the  linear  dynamics.  First, 


the  velocities  in  the  body-fixed  frame  can  be  represented  by 

xB  =  Uq  +  u,  yB  =t=  vq  +  v1  and  zB  =  wq  +  w.  (10) 

Next,  the  trimmed  pitch  angle  0  can  be  defined  as 

o  =  e-e  o.  (ii) 

From  these  relationships,  the  kinematics  equations  (4)  are  now  well  defined. 
The  dynamics  can  be  represented  by 

x°(i)  =  fD(xD(t),xA(t),u(t))  =  Axl(t)  +  Bu(t),  (12) 

where  A  <E  Rllx13,  B  6  Rllx4,  and 

x;  =  [it  v  p  q  axs  bu  w  r  rfb  c  d  <p  0]T.  (13) 


The  helicopter  hovering  mode  is  known  to  be  an  unstable  equilibrium, 
which  means  that  any  small  perturbation  wrill  cause  the  vehicle  to  diverge 
from  the  hovering  condition.  On  the  contrary,  the  helicopter  dynamics  in 
the  forward  flight  condition  are  stable  with  respect  to  a  given  pitch  angle, 
and  converge  to  a  fixed  point  in  the  state  space. 
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Table  1:  Comparison  of  Froude  numbers 


Rotor  Radius 
(ft) 

Rotor  Speed 
(rad/s) 

N 

Froude 

Number 

R22 

13 

53 

0.38 

1134 

Virtual  Model 

10 

62 

0.5 

1194 

R-50 

5 

89 

1 

1230 

2.3  Scaling  Based  on  Froude  Number 


In  order  to  test  the  algorithm  we  propose  in  the  next  section  with  a  hetero¬ 
geneous  helicopter  team,  we  needed  to  generate  a  model  that  has  different 
dynamics  characteristics  from  our  existing  Yamaha  R-50  model.  Since  it  is 
extremely  difficult  to  perform  an  identification  flight  in  a  cruise  condition 
without  a  wind  tunnel  facility,  we  used  the  scaling  technique  presented  by 
Mettler  [23]. 

The  Froude  number  is  the  ratio  of  inertia  to  gravitational  forces.  If  two 
different  models  have  Froude  numbers  that  are  close  each  other,  it  means 
that  two  systems  have  similar  dynamic  properties.  The  number  is  defined 

by 

V2 

F  =  t-,  (14) 


where  V  is  the  characteristic  velocity,  L  is  the  characteristic  length,  and  g  is 
gravitational  acceleration.  In  helicopter  dynamics,  the  rotor  tip  velocity  and 
the  rotor  radius  are  used  as  V  and  L  respectively.  The  Table  1  shows  the 
comparison  of  the  Froude  numbers  of  Yamaha  R-50  and  Robinson  R22,  and 
they  are  very  close.  We  have  created  a  virtual  model  in  the  region  between 
the  Robinson  R22  and  Yamaha  R-50,  which  has  two  twice  the  rotor  diameter 
of  the  R-50  and  has  a  similar  Froude  number.  The  scale  N  refers  to  a  model 
helicopter  with  l/N  the  rotor  diameter  of  the  Yamaha  R-50.  It  should  be 
noted  that  the  relationship  of  the  Froude  number  imposes  a  relation  between 
time  scales  [23], 


(15) 


Based  on  these  comparisons,  the  scaling  of  coefficients  in  A  and  B  (12)  can 
be  done  using  dimensional  analysis. 

Figure  2  and  3  shows  frequency  responses  of  the  R-50  cruise  model  and 
the  scaled-up  virtual  model.  As  expected,  the  attitude  dynamics  of  the 
virtual  model  are  much  slower  than  those  of  the  R50.  This  fact  can  be 


8 


Figure  2:  Frequency  response  from  lateral  cyclic  pitch  uj  to  l>ody  roll  rate 
p  (solid:  virtual  model,  dashed:  R-50  model) 


reconfirmed  by  step  responses  of  attitude  and  body-velocity  dynamics  (Fig¬ 
ure  4). 

3  Summary  of  Results:  MPC-Based  Helicopter 
Formation  Flight 

Model  predictive  control  (MPC),  also  known  as  moving  horizon  or  reced¬ 
ing  horizon  control  (RHC),  has  been  a  useful  technique  for  the  control  of 
slow  dynamical  systems  such  as  chemical  processes  because  such  a  scheme 
requires  high  computational  speed  of  the  control  hardware  due  to  its  on¬ 
line  nature.  With  the  rapid  development  of  digital  processors,  powerful  and 
inexpensive  controllers  make  it  possible  to  adopt  MPC  in  hard  real-time 
applications  [31]. 

MPC  can  provide  a  better  performance  in  controlling  uncertain  plants 
since  it  can  update  the  gain  of  the  controller  based  on  the  current  states, 
while  fixed-gain  control  algorithms  cannot  21  The  capability  to  manipu¬ 
late  the  state-dependency  of  the  control  weighting  matrices  and  constraints 
in  real-time  is  the  key  feature  of  a  model  predictive  control  algorithm.  There 
are  excellent  survey  papers  describing  the  development  of  MPC  theories.  See 
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Figure  3:  Frequency  response  from  longitudinal  cyclic  pitch  U2  to  body  pitch 
rate  q  (solid:  virtual  model,  dashed:  R-50  model) 


22*  and  3  for  example. 

As  long  as  an  MPC  algorithm  is  applied  to  a  formation  flight  problem,  a 
centralized  approach  is  not  a  feasible  choice  at  all,  since  it  is  not  scalable  from 
the  viewpoint  of  computation  and  communication  7  . 1  Tims  it  is  natural  to 
consider  a  decentralized  approach  to  the  formation  flight  problem.  However, 
wntli  a  decentralized  MPC  scheme,  it  is  recognized  that  the  stability  proof 
becomes  very  difficult. 

Under  the  assumption  that  the  dynamics  of  each  vehicle  are  decoupled,  a 
major  obstacle  in  proving  the  stability  of  a  decentralized  MPC  scheme  arises 
in  predicting  neighbors’  behaviors  over  the  future  horizon.  Without  consid¬ 
ering  in  ter- vehicle  constraints,  the  coupling  between  agents  appears  in  the 
performance  index  as  a  penalty  on  relative  gap  errors.  If  there  is  no  appro¬ 
priate  predictions  of  the  behaviors  of  neighbors,  it  is  difficult  to  set  bounds 
on  them.  In  an  attempt  to  resolve  this  difficulty,  authors  of  [7]  introduced 
so  called  ‘compatibility  constraints’,  which  restrict  the  future  variations  of 
neighbors1  optimal  inputs  from  the  previous  optimal  ones.  Using  this,  it  can 

4  In  terms  of  communication,  the  amount  of  communication  in  a  decentralized  setup  can 
be  more  than  that  of  a  centralized  version  depending  on  the  type  of  numerical  procedures 
for  achieving  an  optimal  solution.  This  subject  is  discussed  in  Section  3  A  more  detail. 
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(a)  roll  rates  in  body  coordinates,  (b)  lateral  velocities  in  hotly  coord i- 
lateral  cyclic  step  input  nates,  lateral  cyclic  step  input 


(c)  pitch  rates  in  body  coordinates,  (d)  longitudinal  velocities  in  body 
longitudinal  cyclic  step  input  coordinates,  longitudinal  cyclic  step 

input 


Figure  4:  Step  response  comparisons  between  the  R-50  (dashed)  and  the 
virtual  model  (solid) 
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be  proved  that  the  closed-loop  states  converge  to  the  neighborhood  of  ori¬ 
gin.  However,  due  to  the  nature  of  this  constraint,  once  an  open-loop  control 
is  computed  and  applied  to  the  system  on  the  current  sampling  time,  the 
control  at  the  next  sampling  time  is  constrained  by  the  previous  open- loop 
control.  In  nominal  situations  (no  model  errors  without  exogenous  distur¬ 
bances),  this  may  not  be  a  problem,  since  the  open-loop  control  will  predict 
the  system  behaviors  exactly  and  the  system  may  stay  on  the  optimal  tra¬ 
jectory,  However,  if  the  system  trajectory  starts  to  deviate  from  the  initial 
optimal  trajectory  because  of  disturbances  or  model  errors,  this  constraint 
would  limit  the  effect  of  feedback,  and  the  robust  nature  of  the  feedback 
system  might  be  lost.  Before  the  stability  is  affected  by  uncertainties,  the 
algorithm  may  have  trouble  maintaining  the  feasibility  of  the  optimization 
problem.  Furthermore,  since  this  algorithm  is  applicable  only  to  (nonlinear) 
double  integrator  systems,  it  is  impossible  to  use  this  algorithm  for  forma¬ 
tion  flight  of  helicopters,  which  have  unmeasurable  hidden  state  variables 
related  to  flapping  and  stabilizer  bar  dynamics. 

In  [19],  researchers  used  the  hierarchical  decomposition  method,  which 
decomposes  the  original  formation  graph  into  overlapping  subgraphs  with 
different  hierarchy  levels.  Under  this  decomposition,  the  algorithm  allows 
a  vehicle  at  a  node  with  high  priority  to  compute  control  laws  for  vehicles 
with  lower  priorities,  and  transmit  them  to  vehicles  with  lower  priorities, 
assuming  a  one  time  step  communication  delay  By  doing  this,  since  fu¬ 
ture  behaviors  of  neighboring  vehicles  with  lower  priorities  are  completely 
known  to  vehicles  with  higher  priorities,  'prediction’  is  no  longer  needed,  and 
stability  can  be  proved  by  standard  Lyapunov  arguments.  In  theory,  this 
method  provides  a  simple  and  clear  way  to  prove  the  stability  of  a  decentral¬ 
ized  MPC  scheme  , minimizes  the  conservatism  and  required  communication 
bandwidth.  However,  in  this  case,  the  integrity  of  the  system  structure  is 
totally  dependent  on  the  communication  link,  which  can  be  deteriorated 
easily  in  battlefields. 

Instead  of  sticking  to  proving  stability  of  a  decentralized  MPC,  our  focus 
is  on  designing  an  MPC-based  velocity  tracking  controller  with  penalties 
on  relative  gap  errors,  and  study  the  propagation  of  external  disturbances 
through  the  formation.  In  the  following  sections,  we  suggest  two  strategies 
of  defining  relative  gap  errors  between  neighboring  vehicles,  and  compare 
their  disturbance  attenuation  performance  in  three  dimensional  helicopter 
formation  simulations. 
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3.1  Formation  Topology  and  Definitions  of  Gap  Errors 

In  the  following  discussion,  we  examine  formations  where  each  agent  in  the 
formation  has  connections  that  are  less  than  or  equal  to  two.  Although  cases 
where  one  or  more  agents  in  the  formation  has  more  than  three  connections 
can  still  be  accounted  for,  these  are  considered  special  cases,  and  are  not 
described  here.  In  real-world  operations,  most  helicopter  formations  fall  into 
the  category  with  a  maximum  two  bidirectional  connections  on  each  agent 
[15].  Some  publications  [8,  19]  on  the  vehicle  formations  using  distributed 
MPC  algorithms  consider  arbitrary  formation  shapes  represented  by  con¬ 
nected  graphs.  However,  an  arbitrary  formation  shape  is  obviously  not  used 
in  high-speed  cruise  formations,  especially  for  manned  helicopters.  We  be¬ 
lieve  that  the  research  on  arbitrarily  coupled  vehicle  formation  needs  to  be 
developed  in  the  context  of  behaviors  of  a  cswamr  of  unmanned  vehicles  1]. 

Most  vehicle  formation  algorithms  8,  19,  26,  29]  use  a  so-called  ‘constant 
gap?  strategy  as  described  in  Figure  5.  l\_ li  €  M3  denotes  the  constant 
relative  gap  vector  from  i  —  1-th  vehicle  to  i-th  vehicle  in  the  reference  coor¬ 
dinates,  which  is  represented  by  xr  —  yr  (tangential-normal  to  the  reference 
velocity  Vf)  in  the  figure.  Note  that  we  need  the  relationship 


-  -  V i  (16) 

for  the  unique  definition  of  the  formation  shape.  Also,  we  can  obtain  the 
using  the  rotation  matrix  Rr~rS('ipr)  such  that 

=  (17) 


As  shown  in  Figure  5,  the  gap  error  can  be  defined  as 


1 


S-l 


-  X?  +  l 


i—l,i 


S-l 


If, 


id  1 


(18) 


The  other  type  of  the  gap  strategy  is  called  a  ‘varying  gap’  strategy  (Fig¬ 
ure  6).  In  this  strategy,  the  i-th  vehicle  considers  the  middle  point  in  the 
line  connecting  i  —  1-th  vehicle  to  i  +  1-th  vehicle  as  the  reference  point.  The 
error  vector  becomes 


s  _  xf-l  +'xf+l  _  s 

ei  -  2  • 


(19) 


For  vehicles  in  edges,  the  constant  gap  strategy  (18)  is  used,  although  other 
vehicles  use  the  varying  gap  strategy.  By  using  the  constant  strategy  in 
edges,  the  sum  of  squares  of  gap  errors  is  zero  if  and  only  if  all  the  gap 
errors  are  zero,  even  if  we  use  the  varying  gap  strategy. 
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Figure  5:  Error  vector  definition,  constant  reference  gap 


Figure  6:  Error  vector  definition,  varying  reference  gap 
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Figure  7:  Configuration  of  the  3D  Vee  formation 


The  definition  of  the  error  vector  also  varies  according  to  the  role  of  a 
vehicle  in  the  formation,  e.g.,  leader  vehicle.  Suppose  that  a  vehicle  with 
index  i  is  leading  a  three-dimensional  Vee  formation  (Figure  7).  Then,  the 
objective  of  the  leader  is  to  maintain  constant  gaps  in  zr  and  zs  direction 
from  the  nearest  neighbors,  while  it  uses  the  varying  gap  strategy  in  yr 
direction.  Let  us  define  indices  of  the  nearest  neighbors  of  and  of  such  that 


x  J*-l 

if 

W -w 

H 

VI 

-*i+l 

\i  +  l 

otherwise 

X  /*'—  1 

if  l^f—  zf-\\ 

VI 

-  7S 

Zi+\ 

h  +  1 

otherwise 

The  gap  error  vector  for  the  leader  is  finally  defined  jis 


e,  = 


5(2/, r-i  +  2/1Y1) 


(21) 


where  /|{x,y,c}  denotes  the  corresponding  element  of  a  vector  L  The  gap 
error  vector  for  the  leader  is  illustrated  in  Figure  S.  Note  that,  although  the 
constant  gap  strategy  is  used  in  xr  and  zr  directions,  the  overall  definition 
of  the  gap  error  vector  is  time- varying,  since  a*  and  at  arc  not  constant 
with  respect  to  time. 
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By  using  gap  error  vector  definitions  represented  above,  we  can  imple¬ 
ment  most  of  real-world  helicopter  formations.  For  example,  Vee,  wedge, 
left  and  right  echelon,  and  left  and  right  staggered  formations  [15]  can  be 
realized. 


3.2  Model  Predictive  Control  Law  for  Helicopter  Formations 
Recall  the  system  equation  of  the  i-th  vehicle 

*i(*)  =  /f(x((0,  u((0),  (22) 

where 

x,  €  C  R"? ,  U|  eUtC  R7I« .  (23) 

Here  Xi  and  Ux  are  convex  sets.  We  assume  that  nt(t)  is  measurable,  and 
fi  :  A\  x  Hi  — ►  Rn*  satisfies  standard  conditions  for  admitting  a  unique 
solution.  Remember  previously  defined  this  nonlinear  helicopter  model  as 
a  combination  of  six  degree-of- freedom  nonlinear  kinematics  and  linear  for¬ 
ward  cruise  dynamics  in  Section  2. 
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The  performance  index  </*(-)  has  the  form  of 

/r+L 

£i(xi(t),  x£*  (<),Ui  (t) ,  y f  (t))dt+v/  (xj  ( t+L ) ,  x£i  (r+L) ,  y£  (r+L) ) , 

(24) 

where  the  terminal  penalty  function  v/ (■)  :  Xi  x  AL<  x  K”*  — *  R  is  positive 
definite.  The  subscript  —i  denotes  indices  of  neighbors  of  the  i-th  vehicle, 
yf  :  R  — >  is  the  reference  vector  in  the  spatial  frame,  which  has  (at 

least)  the  reference  velocity  vector  xf,r(t)  and  the  reference  heading 
The  running  cost  C%  :  ?£%  x  AL,*  x  Ui  x  R  — >  R+  has  the  property  that 

£4(0,0,0,05  =  0. 

The  finite  horizon  optimal  control  (FHOC)  problem  with  initial  condi¬ 
tion  x*  =  x*(r)  and  horizon  length  L  is  defined  as 

=  min  Ji(x4,Ki,i),  (25) 


which  is  subject  to  (22),  and  (23). 

Ki  is  a  piecewise  continuous  time-dependent  function  in  open-loop  strat¬ 
egy  space  such  that 


£  Klj  —  {k  :  0,L  x  (26) 

u,(t)  =  (27) 

If  an  optimal  solution  of  the  FHOC  problem  exists,  let  K*(t  —  r,  x *)  denote 
the  solution  for  t  G  [r,  r  +  L\.  Note  that  Vi(xi,t)  =  /(x*, 

Based  on  these,  the  receding  horizon  control  law  for  i-th  vehicle  at  t  =  r 
is  defined  as 

U»(t)  -  =  K-(0,Xi).  (28) 

The  running  cost  A(0  fias  the  form  of 

A(xi,x£j,u„y£)  =  £fop(xi,x£i)  +£f(Xi,yf)  +£f(xj)  +  £“(ui),  (29) 


where  —i  represents  index(es)  of  neighboring  vehicle(s)  following  the  nota¬ 
tion  of  [7].  In  case  of  the  constant  gap  strategy, 


£fp(x*,x£i)  =  ||e^_1||<51<_1  +  \\4i+1\\Qiii 


+  1} 


(30) 


where  ||ar||g  denotes  a  matrix  weighted  norm  ( xTQx ).  Similarly,  using  the 
varying  gap  strategy,  the  term  can  be  represented  as 

£f,p(x»,x£<)  =  HefllQj.  (31) 


17 


In  order  to  predict  x^(*)  over  the  horizon,  we  use  simple  extrapolation 
strategy,  which  is  described  in  Section  3.4. 

£?(•)  penalizes  the  tracking  error,  and  can  be  defined  as 

£?(*».  yf(0)  =  l|y£(0  (32) 


where  C?  :  Xi  — ►  Rn?  maps  the  state  vector  into  corresponding  output  sig¬ 
nals.  In  order  to  track  the  reference  velocity  vector  and  heading  in  the  spatial 
coordinate  system,  we  use  the  following  definition  of  in  simulation: 


C?(xi)  = 


A 


(33) 


The  term  £f  (•)  is  for  remaining  terms  in  the  state  vector  that  do  not  appear 
in  the  previous  running  costs,  <j>,  8 ,  p,  q  and  r,  for  example.  It  is  noticeable 
that  internal  states,  a,  6,  r c,  and  d,  are  not  penalized,  since  they  are 
not  measurable,  and  related  dynamics  are  well  damped.  Therefore,  we  can 
define  £f(*)  such  that 

Cf(^  =  \\C?xi\\Qx,  (34) 

where  Cf  €  Xn? ,  and  nf  denotes  the  number  of  terms  in  the  state 

penalized  by  Cf  (*).  Cf(-)  penalizes  input  magnitudes, 


with  positive  definite  matrix  R  € 

Finally,  the  terminal  penalty  V/(*)  is  defined  by 


(35) 


f)  =  \ 


e5 

1 

e*t+i 

yf-Cf(Xi) 

Cf*i 

ef 

y  ?-<%(*) 

Cf*i 

if  constant  gap  strategy  is  used 


if  varying  gap  strategy  is  used 


(36) 

where  Pz  €  R(n^+ni +rif  )  -s  a  pOSitive  definite  matrix.  Note  that, 

for  a  follower  that  has  two  neighbors,  the  dimension  of  the  gap  error  vector 
n9  =  6  when  the  constant  gap  strategy  is  used,  and  n9  =  3  when  the  varying 
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gap  strategy  is  used.  This  terminal  penalty  plays  several  important  roles 
in  achieving  guaranteed  stability.  In  the  next  section,  we  discuss  a  design 
procedure  of  an  MPC  without  time  dependent  terms  Cgap{‘)  and  Cy{ •). 


3.3  Design  of  Nonlinear  MPC  without  Inter-agent  Couplings 

Due  to  pioneering  research  efforts  in  the  1990s,  several  design  methodologies 
for  stable  MPC  algorithms  are  now  available  [22].  In  most  of  the  stability 
proofs  of  MPC  algorithms,  the  Tail’  of  the  value  function  of  an  FHOC  prob¬ 
lem  plays  a  very  important  role,  since  it  is  known  that,  if  one  can  approx¬ 
imate  this  term  properly,  the  MPC  based  on  FHOC  problem  realizes  the 
virtues  of  infinite- horizon  problems  in  stability  and  robustness.  The  work  of 
Chen  and  Allgdwer  [5]  achieves  this  by  taking  advantage  of  the  terminal  in¬ 
equality  constraint  and  (virtual)  terminal  linear  controller.  Their  technique 
for  proving  stability  is  one  of  the  well-regarded  among  various  finite-horizon 
based  online  optimization  controllers,  including  a  decentralized  MPC  algo¬ 
rithm  [7].  However,  it  is  reported  that  by  introducing  a  terminal  inequality 
constraint  in  the  FHOC  problem  the  numerical  computation  becomes  slow 
18],  and  sometimes  the  MPC  structure  nonrobust  [12]. 

On  the  other  hand,  Jadbabaie  et.  cd.  17]  achieved  a  stable  MPC  algo¬ 
rithm  by  using  the  so-called  control  Lyapunov  function  (CLF)  as  a  terminal 
cost  without  any  terminal  constraints.  In  this  case,  even  though  it  is  not 
easy  to  find  a  proper  CLF  for  a  given  nonlinear  system  without  conser¬ 
vatism,  the  scheme  effectively  minimizes  the  number  of  constraints  subject 
to  FHOC  problem,  which  is  quite  important  in  practical  implementation  of 
an  MPC  algorithm. 

In  the  remaining  section,  we  describe  the  procedure  to  define  a  CLF  for 
the  nonlinear  helicopter  cruise  model  without  inter-agent  coupling  and  time 
dependent  terms  using  semi-definite  programming,  which  appears  in  [18]. 

First,  we  need  to  redefine  helicopter  cruise  dynamics  (Eq.(12))  without 
the  spatial  position  vector  part  as  follows 


d_ 

dt 


Ad  Aa' 
AR“~*S(xa)  0 


(37) 


where  A  in  Eq.(12)  is  separated  into  AA  and  AD,  and  AjR5^5(X"4)  is  rear¬ 
ranged  version  of  R^S  (Eq,(6))  in  corresponding  order  and  dimension. 

If  we  set  upper  and  lower  limits  of  d>  and  6 ,  then  we  can  get  bounds  of 
those  terms  in  AftB^s(itA).  The  operational  limits  of  attitude  variables  are 
set  to 

-30°  <  <f>  <  30°,  -40°  <  6  <  20°.  (38) 
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The  corresponding  bounds  of  terms  in  AR%^S  (xA)  are 

—0.4195  <  sin^tan^^pi  <  0.4195 
—0.8391  <  cos<£tan0=  P2  <  0.3640 
0.8660  <  cos  <f>  =P3<1 

-0.5  <  -sin<£  =Pa<  0.5 
-0.6527  <  sin  (p sec  0  =  p5  <  0.6527 
0.8660  <  coscfisecQ^  p$  <  1.3054 

Now  we  are  ready  to  convert  the  system  matrix  in  Eq.(37)  into  an  affine 
parameter  varying  matrix  [10]  such  that 


^(P(*))  =  ^  +  X>(*M?>  (39) 

2=1 

where  A, \  is  a  constant  matrix  that  has  only  one  1  on  the  corresponding 
entry  and  zeros  otherwise.  A §  is  the  matrix  that  has  constant  terms  in  the 
system  matrix  of  Eq.(37).  Finally  the  above  parameter  varying  matrix  can 
be  represented  by  a  polytopic  model 

(40) 

where  the  set  Co{*}  denotes  the  set  that  includes  all  possible  convex  combi¬ 
nation  of  its  vertex  elements.  The  conversion  from  Eq.(39)  to  Eq.(40)  can 
be  done  by  the  function  aff 2pol  in  LMI  Toolbox  [10],  and  it  results  in  a 
polytopic  model  with  64  vertices. 

For  a  given  weighting  matrices  (only  for  internal  states,  heading,  and 
attitude  variables  extracted  from  Eq.(29)),  the  minimum  upper  bound  of 
the  value  function  is  the  optimal  value  of  the  following  convex  optimization 
problem  [18]: 

mintr(Z)  (41) 

Y  >0 

~YAf  +  AVY  -BX-  XTBT  YQl>2 
Ql/2Y  -I 

Rl/2x  0 


Yt  =  Y,  ZT  —  z, 


XtR  1/2- 
0 

-I 

Z  I 
I  Y 


<  0 


>  0 


(42) 
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Figure  9:  Magnitudes  of  elements  in  matrix  P 

where  Z  is  a  slack  variable,  Y  —  P~\  and  A”  =  KY  are  the  change  of  vari- 
ables.  From  this  result,  the  terminal  cost  (for  the  system  without  external 
time-dependent  signals)  Ls  defined  by  W (x)  =  Px. 

Due  to  the  size  of  the  given  cruise  dynamics,  the  dimension  of  the 
above  convex  optimization  problem  is  prohibitively  huge.  The  problem  has 
2 (n*)2  —  ( nx  -  l)(nr  —  2)  +  nzn *  (292  in  our  case)  variables  and  64+1+1 
LMI  constraints  whose  dimensions  are  (2 nT  +  nu)2,  (2nx)2,  and  (nx)2,  re¬ 
spectively  Most  of  contemporary  pcrsoual  computers  are  based  on  a  32 
bit  structure,  and  their  maximum  allowable  memory  block  size  is  limited 
to  four  giga  bytes,  wfhich  is  marginal  for  our  problem.  Using  LMITOOL 
[11]  and  SeDuMi  [35],  we  tried  to  solve  the  full-scale  problem,  but  a  solu¬ 
tion  was  not  complete  even  after  96  hours.  Instead,  we  sampled  32  vertices 
from  64  vertices  in  the  definition  of  the  polytopic  system,  and  used  them 
for  CLF  computation.  In  this  case,  the  solver  converges  after  15  hours.  The 
magnitudes  of  elements  in  the  obtained  matrix  are  shown  in  Figure  9. 

For  the  given  CLF  V? (x),  it  is  known  that  there  exist  r  6  R+  such  that 

min(  W(x)  +  xrQx  +  u7  Rw)  <0  for  x  €  (43) 

w+ere  f)r  =  {*  €  RnD+nA|W(x)  <  r},  and  np  and  are  dimensions  of 
xD  and  xA,  respectively. 

Let  x“(£;x)  be  the  optimal  trajectory'  at  t  €  [r,r  +  L]  starting  from 


21 


(j>( 0)  (degree) 

9(0)  (degree) 

ICl 

30 

IC2 

20 

-30 

IC3 

-40 

30 

IC4 

-40 

-30 

Table  2:  Initial  conditions  used  for  single  helicopter  simulations  with  the 
designed  MPC  controller 


x  =  x(r).  If  x*(r  +  L;x)  €  flr,  then  then  the  trajectory  starting  from  x 
converges  to  the  origin  under  the  RHC  scheme  [17]. 

To  complete  the  design  procedure,  we  need  to  choose  a  proper  horizon 
length  L  so  that  we  can  have  a  sufficiently  large  invariant  set  that  includes 
Qr.  However,  in  high-dimensional  systems,  it  is  very  difficult  to  compute 
an  invariant  set  corresponding  to  an  L  analytically  or  numerically  [17].  In 
our  research,  we  set  L  =  0.5  (sec)  based  on  several  simulation  results  with 
various  values  of  L.  The  sampling  frequency  is  set  to  50Hz  (8  =  0.02  sec) 
so  that  it  is  identical  with  that  of  our  existing  UAV  control  system. 

In  order  to  show  the  validity  of  the  CLF  computed  from  the  sub-sampled 
polytopic  set,  computer  simulations  are  performed  with  several  initial  con¬ 
ditions.  As  shown  in  Figure  10-12,  the  designed  MPC  scheme  successfully 
stabilizes  all  the  initial  conditions  in  Table  2. 

It  is  noticeable  that  all  the  states  converge  to  the  origin  in  spite  of  control 
input  saturations  in  the  beginning  of  simulations  (Figure  13).  Even  though 
the  design  procedures  we  use  here  are  originally  for  unconstrained  MPC  [17], 
the  controller  works  well  with  input  constraints  in  our  case. 

3.4  Interagent  Information  Structure  and  Communication 

Provided  that  the  inter-agent  communication  happens  only  one  time  per 
sampling  instance,5  a  decentralized  algorithm  can  be  implemented  with 
lower  bandwidth  communication  channels,  while  a  centralized  setup  requires 
high  bandwidth  communication  channel  on  the  central  agent  which  solves 
the  optimal  control  problem  for  every  agent.  However,  in  a  decentralized 
setup,  if  inter-agent  communications  are  required  during  a  numerical  iter¬ 
ation,  the  total  amount  of  information  transferred  between  agents  can  be 
more  than  that  of  a  centralized  case  [27] .  This  scheme  falls  into  a  category 

5This  means  that  there  is  no  communication  while  solving  a  local  optimization  problem. 
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Figure  10:  Attitude  and  heading  responses.  Note  that  the  pitch  angle  should 
converge  to  the  trim  condition  (-12.6  degree),  not  to  zero.  ICl:  blue  solid, 
IC2:  blue  dashed,  IC3:  red  dash-dotted,  ICM:  red  dotted 


T - — - 1  1 - - - T  > 


0  2  4  6  6  10  12  14  16  16  20 

Figure  11:  Body  velocity  responses,  ICl:  blue  solid,  IC2:  blue  dashed,  IC3: 
red  dash-dotted,  ICM:  red  dotted 
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Tim*  {*«) 

Figure  12:  Body  angular  velocity  responses,  IC1:  blue  solid,  IC2:  blue 
dashed,  IC3:  red  dash-dotted,  IC4:  red  dotte<l 


NumtMr  oi  stop* 

Figure  13:  Control  inputs,  IC1:  blue  solid,  IC2:  blue  dashed,  IC3:  red 
dash-dotted,  IC4:  red  dotted 
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Figure  14:  Values  of  ,  ICl:  blue  solid,  IC2:  blue  dashed,  IC3:  red  dash- 
dotted,  IC4:  red  dotted 


of  algorithms  using  cooperative  iteration  [4].  Since  this  cooperative  itera¬ 
tion  needs  stable  and  high  bandwidth  communication  channels  and  strict 
synchronization  between  agents,  it  is  more  challenging  to  implement  one 
on  a  hard  real-time  system.  In  the  proposed  setup,  we  use  one  inter-agent 
communication  per  every  sampling  instance,  and  there  is  no  communication 
while  solving  an  FHOC  problem. 

As  shown  in  (29),  the  proposed  MFC  scheme  requires  the  predicted 
trajectory  of  neighbors  for  t  e  [ryr  -f  L  at  every  sampling  time.  In  [7], 
neighboring  agents  interchange  their  predicted  trajectories  and  use  them 
as  estimations  of  neighbors’  trajectories.  This  appears  to  be  a  reasonable 
choice,  but  it  requires  higher  communication  bandwidth  than  the  scheme 
that  uses  only  current  neighbors’  states  and  extrapolates  them  for  prediction. 
Moreover,  few  research  is  done  about  the  cases  that  these  predictions  are  not 
accurate  due  to  external  disturbances.  The  interagent  information  structure 
is  one  of  the  most  controversial  subjects  in  distributed  model  predictive 
research,  c.g.  [7,  19]. 

In  our  research,  the  transferred  spatial  positions  of  neighboring  agents 
are  extrapolated  over  the  finite  horizon.  The  predicted  positions  of  ncigb- 


25 


boring  vehicles  axe  represented  by 

x^(r  + 1)  =  x£^(r)  4-  x£*(r)t  for  0  <  t  <  L.  (44) 

As  shown  in  the  following  section,  acceptable  performance  can  be  achieved 
by  this  scheme.  However,  since  the  prediction  error  increases  as  the  predic¬ 
tion  horizon  extends,  the  extension  of  the  length  of  prediction  horizon  does 
not  mean  the  enlargement  of  domain  of  attraction.  In  addition,  the  velocity 
information  transferred  to  neighbors  should  be  properly  filtered  so  that  the 
effects  of  noisy  measurements  can  be  minimized. 

3.5  Simulations 

The  MPC  algorithm  for  autonomous  helicopter  formations  as  formulated 
and  described  above  was  implemented  in  Matlab/Simulink  environment. 
The  core  of  the  implementation  involves  a  solution  to  the  FHOC  problem, 
and  the  following  section  is  devoted  to  a  discussion  of  numerical  algorithms 
for  FHOC  problems. 

3.5.1  Numerical  Solver  for  Finite-Horizon  Optimal  Control  Prob¬ 
lem 

In  general,  the  nonlinear  FHOC  problem,  a  single  player  differential  game 
from  the  viewpoint  of  the  game  theory,  can  be  numerically  solved  in  two 
ways:  indirect  and  direct  approaches.  Indirect  approaches  utilize  the  neces¬ 
sary  conditions  given  by  the  Pontryagin  Minimum  Principle.  Then,  it  can 
be  viewed  as  a  multi-point  boundary  value  problem,  and  an  optimal  solution 
is  obtained  by  boundary  value  problem  solvers  like  shooting  methods  and 
finite-element  methods.  However,  these  indirect  methods  are  known  to  be 
very  sensitive  to  their  initial  conditions,  and  as  a  result,  lack  robustness. 
See  [28]  and  references  therein.  Therefore,  it  is  not  practical  to  use  indirect 
methods  in  solving  online  optimization  problems. 

Direct  methods,  on  the  other  hand,  discretize  continuous  dynamics  and 
cost  functions  using  a  high-order  Runge-Kutta  method  [9,  28  or  direct  collo¬ 
cation  [37],  convert  it  into  a  finite-dimensional  nonlinear  optimization  prob¬ 
lem,  and  obtain  an  optimal  solution  through  nonlinear  programming  tech¬ 
niques.  These  provide  approximate  solutions,  but  they  are  robust  against 
arbitrary  initial  conditions,  and  optimal  solutions  with  reasonable  accu¬ 
racy  can  be  achieved  using  less  intensive  numerical  procedures  than  indirect 
methods. 
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Figure  15:  Right  echelon  formation  with  eight  helicopters  used  in  simulations 

In  the  following  simulations,  we  use  the  DynOpt  package  [9].  This 
package  uses  the  4th  order  Runge-Kutta  method  for  discretization  of  the 
continuous-time  dynamics  and  cost  functions,  and  achieves  an  optimal  so¬ 
lution  using  the  sequential  unconstrained  minimization  technique  (SUMT). 
Since  the  package  contains  the  SUMT  algorithm  and  it  is  tightly  integrated 
with  discretization  procedures,  DynOpt  allows  for  compact  and  versatile 
implementation.  Although  other  solvers  using  direct  method  like  XTG  [24] 
and  DIRCOL  37'  require  an  external  commercial  nonlinear  programming 
solver,  it  is  worthwhile  to  use  them  in  that  they  provide  more  user-friendly 
options  and  detailed  error  messages  for  debugging.  In  the  ease  of  NTG,  it 
was  reported  that  the  package  is  used  as  an  MPC  engine  in  a  hard  real-time 
application  [7]. 

3.5.2  Simulation  Setup 

Figure  15  shows  the  configuration  of  the  right  echelon  formation  [15]  used 
in  simulations.  We  chose  this  configuration  because  we  want  to  investigate 
the  propagation  of  disturbances  through  connected  vehicles. 

As  shown  in  (29),  the  proposed  MPC  scheme  requires  the  predicted 
trajectories  of  neighbors  for  t  €  [r,r  -f  L  at  every  sampling  time.  In  our 
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work,  we  adopt  the  scheme  that  interchanges  the  current  position  and  veloc¬ 
ity  vectors  of  neighbors  and  extrapolates  them  over  the  prediction  horizon. 
Although  this  information  structure  may  prohibit  us  from  using  a  long  pre¬ 
diction  horizon,  since  the  prediction  error  may  grow  over  prediction  horizon, 
it  allows  us  to  minimize  the  inter  agent  communication  burden. 

In  the  case  of  the  constant  gap  strategy,  gap  vectors  are  defined  as 
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and  all  units  are  in  ft.  Note  that,  even  in  the  case  using  the  varying  gap 
strategy,  vehicles  in  far  edges,  vehicle  0  and  vehicle  7  in  our  configuration 
(Figure  15),  using  the  constant  gap  strategy  using  the  above  constant  rela¬ 
tive  gap  vectors. 

When  we  defined  the  FHOC  problem  (Eq.(25)),  we  assumed  that  there 
are  constraints  on  states  such  that  x*  6  In  recent  publications  [12,  13), 
it  is  reported  that  the  state-constrained  MPC  scheme  is  possibly  not  robust 
due  to  the  discontinuity  in  the  value  function  induced  by  state  constraints. 
In  accordance  with  this  observation,  we  use  only  input  constraints  in  our 
application,  even  though  our  FHOC  problem  solver  allows  state  constraints 
in  the  formulation.  We  use  the  following  admissible  input  set  for  all  vehicles 
in  our  simulations 


Ui  =  {uG  Rn?|  -  1  <  uj  <  1,  1  <j<  «?},  (46) 

where  v?  denotes  the  j-th  element  of  the  vector  u. 

A  final  consideration  here  is  the  weighting  on  the  attitude  states,  $  and 
0  (roll  and  pitch  angles).  It  is  well  known  in  the  field  of  aircraft  control  that 
the  stabilization  of  attitude  dynamics  is  a  key  to  good  controller  design.  This 
is  due  to  the  coupling  between  the  translational  dynamics  and  the  attitude 
dynamics.  In  order  to  keep  the  attitude  variation  at  a  minimum,  the  terms 
related  with  <j)  and  0  in  (34)  should  be  more  heavily  penalized  than  other 
terms. 


3.5.3  Comparison  Between  Constant  and  Varying  Gap  Strategies 

In  order  to  compare  disturbance  attenuation  performances  of  two  types  of 
gap  error  strategies,  we  exert  negative  longitudinal  wind  gust  on  the  leading 
vehicle  0.  The  longitudinal  acceleration  induced  by  the  wind  gust  is  shown 
in  Figure  16. 
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Figure  17  shows  comparisons  of  the  relative  gap  responses  between  the 
constant  and  the  varying  gap  strategies.  As  shown  here,  the  MFC  algorithm 
successfully  damps  out  the  relative  gap  errors  caused  by  the  disturbance  as 
they  propagate  into  the  formation  irrespective  of  the  gap  error  strategy 
type.  This  set  of  figures  also  illustrates  an  interesting  feature  of  the  varying 
gap  strategy.  When  the  varying  gap  strategy  is  used,  the  relative  gap  error 
between  the  Vehicle  0  and  Vehicle  1  is  lager  than  that  of  the  constant  gap 
strategy  as  shown  in  Figure  17(a).  This  is  due  to  the  nature  of  the  varying 
gap  strategy.  When  the  Vehicle  0  approaches  Vehicle  1,  then  Vehicle  1 
can  reduce  the  size  of  the  gap  error  penalty  very  rapidly  by  moving  toward 
Vehicle  2.  In  addition,  the  effects  of  reducing  gap  error  penalty  by  the 
evasive  motion  of  the  Vehicle  2  is  amplified  through  the  structure  of  the 
varying  gap  strategy.  Therefore,  the  relative  gap  between  Vehicle  0  and 
Vehicle  1  can  remain  larger  than  in  the  constant  gap  strategy  case.  In 
other  words,  the  varying  gap  strategy  achieves  more  'cooperation’  from  the 
neighboring  vehicle  (s)  in  terms  of  reducing  the  gap  error  penalty.  Although 
the  varying  gap  strategy  shows  poor  performance  in  terms  of  the  gap  error 
between  Vehicle  0  and  Vehicle  1,  Figure  18  shows  that,  in  comparison  with 
the  constant  gap  strategy,  the  varying  gap  strategy  can  reduce  the  maximum 
velocity  fluctuation  of  Vehicle  1,  which  is  directly  related  to  the  passenger 
comfort.  Also,  the  varying  gap  strategy  showrs  superior  performance  from 
the  point  of  view  of  the  disturbance  attenuation  as  the  wind  gust  induced 
disturbance  propagates  tlirough  the  formation  (Figure  17(b)  -  Figure  17(f)). 

The  large  gap  error  of  the  leader  vehicle  can  be  remedied  by  increasing 
the  size  of  penalties  on  tracking  errors.  This  is  natural,  since  it  is  very 
important  for  a  leader  vehicle  to  maintain  its  velocity  and  heading,  and  the 
performance  of  the  entire  formation  can  depend  on  the  tracking  quality  of 
the  leader  vehicle.  For  this  reason,  an  active  disturbance  rejection  scheme 
should  be  considered  for  vehicles  at  formation  edges. 

3.5.4  Performance  with  Heterogeneous  Formations 

In  order  to  test  our  algorithm  in  a  heterogeneous  setup,  we  put  virtual  mod¬ 
els  from  Section  2  in  locations  of  1,  2,  4,  5,  and  6  in  the  right  echelon  for¬ 
mation  (Figure  15).  For  Vehicles  CL  3,  and  7,  the  original  R-50  cruise  model 
is  used.  The  varying  gap  strategy  is  utilized  in  the  following  simulation. 

As  shown  in  Figure  19,  the  proposed  algorithm  successfully  damps  out 
external  disturbance  as  it  propagates  through  the  formation. 

In  the  case  of  mesh  stability  algorithm  [30],  the  gap  error  induced  by 
the  leader  motion  is  amplified  between  a  normal  vehicle  and  a  more  agile 
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Figure  16:  Disturbance  induced  by  wind  gust,  negative  x  direction 


vehicle.  Since  the  algorithm  uses  position  information  of  the  leader  as  well 
as  neighboring  vehicles,  and  an  agile  vehicle  tends  to  maintain  the  relative 
position  from  the  leader  even  when  the  relative  gap  errors  between  neighbors 
become  large,  there  exists  a  ‘jump’  in  gap  error  propagation.  However,  in 
our  MPC-based  formulation,  since  vehicles  share  only  reference  velocities 
and  heading,  the  dilemma  of  the  global  connection  to  the  leader  does  not 
appear,  and  the  space  between  vehicles  can  be  safely  maintained. 

Figure  20  shows  comparisons  of  gaps  between  homogeneous  and  het¬ 
erogeneous  formations.  It  is  noticeable  that  the  proposed  algorithm  shows 
comparable  disturbance  attenuation  capability  in  heterogeneous  formation. 

3.6  Discussion 

The  current  problem  of  the  proposed  scheme  is  that  the  FHOC  problem 
solver,  DynOpt,  is  too  slow  for  real-time  applications.  It  takes  about  an  hour 
to  perform  a  8-vchicle  formation  simulation  for  80  seconds.  Our  algorithm 
will  be  tested  with  different  solvers  such  as  the  gradient  descent  method 
[36].  We  arc  optimistic  because  we  already  have  performed  successful  MPC 
experiments  using  the  gradient  decent  method  431].  The  enhancement  of 
the  performance  of  our  existing  solver  (using  gradient  decent  method)  is 
also  now  being  pursued. 
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(e)  Gap  between  Vehicle  4  and  5  (f)  Gap  between  Vehicle  5  and  6 

Figure  17:  Comparison  of  gaps  in  x-direction,  constant  gap  strategy:  dashed 
lines,  varying  gap  strategy:  solid  lines 
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Figure  IS:  Forward  velocity  of  the  Vehicle  1  under  the  constant  gap  strategy 
(dotted),  and  the  varying  gap  strategy  (solid) 

4  Hardware-In-the-Loop  Simulation  (HILS)  Sys¬ 
tem  for  BEAR  Fleet 

Hardware-in-the-Loop  Simulator  (HILS)  allows  embedded  real-time  modules 
to  be  tested  in  a  simulated  environment  in  closed-loop.  The  necessity  of 
HTLS  system  is  emphasized  especially  in  developing  a  complex  system  that 
requires  high  cost  physical  experiments.  For  example,  the  development  of 
automotive  control  systems  is  also  a  well-known  application  area  of  II1LS 
due  to  its  difficulties  on  real  ear  experiments  [14]. 

Since  the  UAV  system  is  extremely  complicated,  expensive,  and  safety- 
critical,  it  is  natural  to  introduce  a  HILS  system  in  its  development  cycle.  In 
general,  UAV  experiments  involve  high  risks,  and  a  single  typo  in  a  huge  code 
set  can  cause  complete  destruction  of  the  system  and/or  injuries  of  ground 
staffs.  Considering  the  risks  involved  in  inulti-UAV/UGV  experiments  like 
a  formation  flight,  the  entire  development  cycle  may  be  severely  delayed 
without  sufficient  testing  on  a  HILS  system. 

The  first  version  of  the  HILS  system  for  BEAR  fleet  was  implemented 
on  real-time  operating  system  (RTOS)  Vx Works.  In  this  version,  a  flight 
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Figure  19:  Gaps  in  x-direction,  varying  gap  strategy  under  negative  gust  in 
longitudinal  direction 
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(c)  Gap  ixween  vehicle  2  and  3 


(d)  Gap  l>etween  vehicle  3  and  4 


Figure  20:  Comparison  of  gaps  in  x-direction,  homogeneous  formation: 
dashed  lines,  heterogeneous  formation:  solid  lines 
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Figure  21:  HILS  system  structure 


controller  for  Yamaha  RMAX  was  implemented  using  the  time-based  pro¬ 
gramming  language.  Giotto  [16],  and  the  effectiveness  of  Giotto  was  tested 
with  the  HILS  system.  However,  since  the  actual  control  software  for  the 
RMAX  was  not  available  until  2004,  only  a  simplified  version  of  a  controller 
was  implemented.  As  a  result,  the  VxWorks-based  HILS  system  is  not  di¬ 
rectly  applicable  to  the  control  software  running  on  our  flight  platforms. 
Moreover,  it  does  not  include  a  safety  pilot  and  a  ground  station/operator 
in  the  loop. 

Recently,  in  parallel  with  the  development  of  the  Ursa  Maxima  series 
based  on  Yamaha  RMAX  industrial  helicopters,  we  developed  a  HILS  system 
compatible  with  the  existing  navigation  software  in  the  BErkeley  AeRobot 
(BEAR)  program.  In  the  following  sections,  we  review  the  implementation 
of  new  version  of  our  HILS  system. 

4.1  Hardware  Structure 

The  hardware  structure  of  HILS  may  vary  depending  on  each  systems’s  char¬ 
acteristics.  In  the  case  of  development  of  a  missile  seeker  system,  the  sensor 
systems  are  designed  for  a  specific  type  of  missile  and  the  hardware  struc¬ 
ture  is  also  a  part  of  development.  Hence  the  whole  integrated  flight  system 
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System 

Message 

Description 

Frequency  (Hz) 

GPS 

PRTK 

computed  position,  RTK  1 

5 

PXY 

Cartesian  coordinate  position 

1 

INS 

M3 

sensor  status  and  navigation  data 

1 

M3512 

delta  velocity  and  delta  attitude  data 

RMAX 

RX 

RMAX  receiver  data 

40 

YACS 

RMAX  system  status  data 

10 

Table  3:  INS,  GPS,  and  RMAX  messages  used  in  BEAR  avionics 


including  navigation  sensors  and  processors  should  be  tested  in  a  simulated 
harsh  environment*  However,  in  the  case  of  our  UAV  system,  since  it  is  made 
up  of  custom  sensors  and  parts,  and  the  hardware  structure  is  not  changed 
frequently,  the  test  of  entire  avionics  is  not  required.  Rather,  as  a  testbed 
of  various  complex  scenarios,  BEAR  UAV  systems  may  undergo  a  series  of 
modifications  in  its  software  structure.  Thus  the  most  important  feature  to 
be  included  in  the  HITS  system  for  the  BEAR  fleet  is  the  verification  ability 
of  real-time  software  installed  on  the  vehicles.  Also,  it  is  desirable  to  create 
a  simulated  situation  as  close  to  the  real  experiment  as  possible.  In  these 
contexts,  we  include  a  safety  pilot  in  the  loop  as  well  as  major  navigation 
sensors  like  GPS  and  INS  (Figure  21). 

Pilot  commands  and  RMAX  vehicle  status  are  relayed  by  Yamaha  Atti¬ 
tude  Control  System  (YACS)  via  serial  ports.  The  HILS  computer  running 
on  QNX4  generates  various  sensor  outputs  based  on  simulated  helicopter  dy¬ 
namics  and  kinematics.  The  hardware  components  simulated  in  the  HILS 
system  are  CMIGITS-II  INS  by  Systron  Donner  and  OEM3  GPS  card  by 
Novatel.  Note  that,  as  shown  in  Figure  21,  our  HILS  system  includes  the 
vehicle,  the  safety  pilot  and  ground  st  at  ion/ operator  so  that  it  can  create  a 
situation  that  may  happen  in  a  real  experiment. 

4.2  Software  Structure 

The  HILS  system  must  provide  all  the  INS/GPS  and  YACS  messages  used 
by  the  flight  control  system  (FCS).  Table  3  shows  core  messages  required  by 
FCS.  Although  the  FCS  software  uses  time-based  and  event-driven  schedul¬ 
ing,  these  messages  are  purely  time-based.  Especially,  the  timing  of  the 
M3512  message  should  be  kept  precisely,  since  the  basic  scheduling  of  BEAR 
FCS  software  is  depending  on  the  arrival  timing  of  this  message.  The  soft¬ 
ware  timers  provided  by  RTOS  QNX4  kernel  are  used  for  these  timings,  and 
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TVnotwc) 

Figure  22:  RS-232  puLse  trains  on  IXS  M3512  channel 


the  timing  performance  of  M3512  is  measured  by  an  oscillascope.  Figure  22 
shows  the  measured  RS-232  pulse  trains  of  M3512.  The  average  period  be¬ 
tween  these  pulse  trains  is  approximately  10*24  ms  (97.7  Hz)  and  timing 
jitter  is  less  than  0.05  ms,  which  are  small  enough  for  our  application. 
The  entire  software  structure  of  the  IIILS  system  is  shown  in  Figure  23. 
The  HILS  system  consists  of  five  processes:  server,  ins.gps,  rmax.io,  and 
simulator,  server,  which  is  not  shown  in  the  figure  for  simplicity,  is  the 
parent  process  of  all  other  processes,  and  creates  shared  memories.  This  will 
be  used  as  a  gateway  between  the  HITS  control  station  and  the  HILS  system 
In  future  development.  The  core  computation  parts  of  the  above  processes 
are  ported  from  Vx Works  version  [16]  to  QNX4.  Timing  and  shored  memory 
structures  are  newly  designed. 

Process  names  are  self-explanatory',  simulator  Integrates  helicopter  dy¬ 
namics  and  kinematics  using  Rnnge-Kutta  4-th  order  formula,  and  updates 
the  shared  memory  shm.sim.dat a  at  100  Hz.  The  process  gps  creates  pxy 
and  prtk  messages  based  on  simulation  results  and  several  coordinate  con¬ 
versions.6  Likewise,  ins  creates  M3512  and  M3  messages  using  simulation 

6 For  prtk  message,  we  need  a  transformation  from  Cartesian  coordinate  to  geodetic 
(Longitude- Latitude- Height,  LLH)  coordinate.  Also.  LLH  coordinate  to  Earth-Centered- 
Earth-Fixed  (ECEF)  coordinate  conversion  is  needed  for  calculation  of  local  Cartesian 
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Figtrre  23:  H1LS  system  software  structure 

results  and  GPS  conversion  results,  rmax.io  deals  with  communications  be¬ 
tween  the  HILS  system  and  YACS/FCS.  In  order  to  prevent  simultaneous 
read/write  access  on  a  same  shared  memory,  a  mutually  exclusive  memory 
access  structure  is  implemented  using  a  memory  access  register  in  shared 
memory. 

For  more  realistic  sensor  simulations,  it  is  necessary  to  add  proper  mea¬ 
surement  noises  to  the  solutions  from  simulator.  Based  on  2]  and  data 
from  experiments,  properly  scaled  Gaussian  noises  are  added  to  INS  solu¬ 
tions.  For  GPS  solutions,  normally  distributed  uncorrelated  random  noises, 
whose  standard  deviation  is  equals  to  0.02/3  (in  meter  scale)  [25],  are  added 
to  the  solution. 

4.3  Simulation  Results 

As  an  example  of  HILS  system  application,  a  waypoint  navigation  using 
Vehicle  Control  Language  (VCL)  [33]  in  batch  mode  is  performed  on  the 
proposed  HILS  system.  Figure  24  and  Figure  25  show  vehicle  states  during 

origin. 
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(c)  body  rates 


(d)  attitude  and  yaw  angle 


Figure  24:  FULS  simulation  results  during  a  waypoint  navigation 


the  simulation  and  the  vehicle  trajectory  in  two-dimensional  plane,  respec¬ 
tively. 


5  Conclusions  and  Future  Work 

In  this  ARO  STIR  W911NF-04- 1-0448,  the  problem  of  autonomous  heli¬ 
copter  formations  is  considered.  A  stable  MFC-based  controller  for  a  single 
helicopter  was  implemented  first,  and  then  carefully  designed  inter- vehicle 
coupling  terms  were  added  in  order  to  maintain  safe  space  between  heli¬ 
copters.  In  Chapter  3,  we  showed  the  proposed  algorithm  successfully  damps 
out  exogenous  disturbances  via  a  series  of  simulations.  The  algorithm  was 
also  applied  to  a  heterogeneous  formation,  and  it  showed  a  good  attenuation 
property. 

The  simulations  of  the  MPC-based  formation  flight  scheme  proposed 
throughout  this  report  show  that  the  algorithm  involves  more  active  and 
dynamic  scenarios  than  the  case  of  mesh  stability  30  .  It  means  that  the 
experimental  verification  of  the  algorithm  will  be  more  challenging,  and  more 
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Figure  25:  Waypoint  navigation  in  the  IIILS  environment 
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dangerous.  In  order  to  minimize  the  possibility  of  an  unfortunate  accident, 
newly  implemented  softwares  should  be  strictly  verified  before  the  actual 
experiment,  and  all  the  experiments  should  be  carefully  designed  considering 
the  safety  of  ground  crew.  Our  HILS  system  will  play  very  important  role 
in  future  development  and  experiments. 

As  the  continuing  effort  of  this  project,  we  are  now  developing  the  con¬ 
cept  of  “formation  manager”  which  is  the  high-level  agent  of  the  MPC  con¬ 
troller  enabling  more  dynamic  and  flexible  autonomous  formation  flights. 
Also,  the  developed  algorithms  are  now  being  implemented  on  real-time 
hardware  and  will  be  tested  under  HILS  environment  before  the  real-world 
experiments.  All  this  work  is  currently  supported  by  Phase  I  of  the  ARO 
STTR  A05-T011  which  ends  in  February  2006. 
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